d = [3 5 4 7 6 11];

Aeq =  [1 0 0 0 0 0  1 0 0 0 0 0  0 0 0 0;
        0 1 0 0 0 0  0 1 0 0 0 0  0 0 0 0;
        0 0 1 0 0 0  0 0 1 0 0 0  0 0 0 0;
        0 0 0 1 0 0  0 0 0 1 0 0  0 0 0 0;
        0 0 0 0 1 0  0 0 0 0 1 0  0 0 0 0;
        0 0 0 0 0 1  0 0 0 0 0 1  0 0 0 0];
beq = d.';
A = [1 1 1 1 1 1 0 0 0 0 0 0  0 0 0 0;
     0 0 0 0 0 0 1 1 1 1 1 1  0 0 0 0];
B = [20; 20]; % 列
LB = [zeros(12, 1); -inf; -inf; -inf; -inf]; % 列
UB = [];
% X0 = rand(16, 1); % 起始搜索点
X0 = [3 5 0 7 0 1 0 0 4 0 6 10 5 1 2 7].';

[optx, fval] = fmincon(@fun, X0, A, B, Aeq, beq, LB, UB); 


function COST = fun(X)
    a = [1.25 8.75 0.5 5.75 3 7.25];
    b = [1.25 0.75 4.75 5 6.5 7.75];
    x = [X(13), X(15)];
    y = [X(14), X(16)];
    for i = 1:6
        for j = 1:2
            dij(i, j) = sqrt((a(i)-x(j))^2 + (b(i)-y(j))^2);
        end
    end
    D = [dij(:, 1); dij(:, 2); 0; 0; 0; 0];
    COST = (D.')*X;
%     f1=0;
%     for i = 1:6
%         s(i) = sqrt((X(13)-a(i))^2 + (X(14)-b(i))^2);
%         f1 = s(i)*X(i)+f1;
%     end
%     f2=0;
%     for i = 7:12
%         s(i) = sqrt((X(15)-a(i-6))^2 + (X(16)-b(i-6))^2);
%         f2 = s(i)*X(i)+f2;
%     end
%     COST=f1+f2;
end

